package com.neuronrobotics.aim.gui;

import java.awt.Color;
import java.awt.Font;
import java.awt.Rectangle;
import java.awt.SystemColor;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.text.DecimalFormat;
import javax.swing.JButton;
import javax.swing.JFrame;
import javax.swing.JLabel;
import javax.swing.JOptionPane;
import javax.swing.JPanel;
import javax.swing.JSeparator;
import javax.swing.JSlider;
import javax.swing.JTable;
import javax.swing.JTextField;
import javax.swing.border.BevelBorder;
import javax.swing.table.DefaultTableModel;
import com.neuronrobotics.aim.robot.ProstateRobotKinematicModel;
import com.neuronrobotics.openigt.OpenIGTPanel;
import com.neuronrobotics.sdk.genericdevice.GenericPIDDevice;
import com.neuronrobotics.sdk.pid.PIDCommandException;
import javax.swing.JRadioButton;
import javax.swing.JCheckBox;

public class ImageControlPanel extends JPanel {
	private static final long serialVersionUID = 3485775714533234856L;
	
	/** The desired joint values. */
	private int[] desiredJointTicks = null;
	/** The update. */
	// TODO , what is this warning
	@SuppressWarnings("unused")
	private boolean update = false;
	
	private JLabel jLabel = null;
	private JSlider jSlider = null;
	// public boolean isDesiredTipPositionCalucated =false;
	// TODO check the array dimension carefully
	public double[] desiredJointSpaceVector = new double[5];
	private double[] desiredCartesianSpaceVector = new double[3];
	// this is used to store calculated desired joint angles (double)
	private double[] desiredJointTicksDouble = new double[5];
	public int[] desiredJointTicksInteger = new int[5];

	private ProstateRobotKinematicModel robotKinematics = new ProstateRobotKinematicModel();

	private JButton jButtonCalculcateInvKinematics = null;
	public static EventResponseTable tableZFrameRegistration;
	private JTextField textFieldXAxisDesiredJointPosition;
	private JTextField textFieldYAxisDesiredJointPosition;
	private JTextField textFieldZAxisDesiredJointPosition;
	private JTextField textFieldInsertionDesiredJointPosition;
	private JTextField textFieldRotationDesiredJointPosition;
	private JTextField textFieldRetractionDesiredJointPosition;

	public JTextField textFieldXAxisJointPosition = null;
	public JTextField textFieldYAxisJointPosition = null;
	public JTextField textFieldZAxisJointPosition;
	public JTextField textFieldInsertionJointPosition;
	public JTextField textFieldRotationJointPosition;
	public JTextField textFieldRetractionJointPosition;

	private JSeparator separator;
	private JSeparator separator_1;
	private JLabel lblZFrameRegistrition;

	public static EventResponseTable tableDesiredNeedleRAS;
	private JLabel lblDesiredNeedleRas;

	public static EventResponseTable tableBaseinZFrameCalibration;

	public JTable tableActualNeedleRAS;
	private JLabel lblActualNeedleRas;
	private JLabel lblActualNeedleRas_1;

	private JLabel lblRoll = null;
	private JLabel lblPitch = null;
	private JLabel lblYaw = null;
	private JLabel lblPosX = null;
	private JLabel lblPosY = null;
	private JLabel lblPosZ = null;
	private JLabel lblCartesianError = null;
	private JLabel lblJointError = null;

	public JTextField textFieldXAxisJointError = null;
	public JTextField textFieldYAxisJointError = null;
	public JTextField textFieldZAxisJointError = null;
	public JTextField textFieldInsertionJointError = null;
	public JTextField textFieldRotationJointError = null;
	public JTextField textFieldRetractionJointError = null;

	private JButton jButtonInsertNeedle = null;
	private JButton jButtonStop = null;

	private JTextField textFieldXAxisNeedleCartesianError = null;
	private JTextField textFieldYAxisNeedleCartesianError = null;
	private JTextField textFieldZAxisNeedleCartesianError = null;
	private JTextField textFieldRollNeedleCartesianError = null;
	private JTextField textFieldXPitchNeedleCartesianError = null;
	private JTextField textFieldYawNeedleCartesianError = null;
	private JButton jButtonRetractNeedle = null;
	private JButton jButtonRetractStylet = null;
	private JButton jButtonHomePosition = null;
	public static JButton jButtonZFrameRegistration = null;
	public static JButton jButtonBaseinZFrameCalibration = null;
	private JButton jButtonDesiredNeedleRAS = null;

	private MainGUI kinematicsGUI;
	public JTextField textFieldInsertionVelocity;
	private JTextField textFieldInsertionAcceleration;
	public JTextField textFieldNeedleRotation;
	public JTextField textFieldNeedleRotationVelocity;

	public boolean clockwiseEnabled = true;
	private double desiredInsertionVel;
	private double desiredInsertionAccel;
	private double desiredRotationAngle;
	private double desiredRotationVel;

	private boolean enableRobotMove = false;
	private boolean enableHomePosition = false;
	private boolean enableInsertion = false;
	private boolean enableRetraction = false;
	private boolean enableRotation = false;
	private boolean insertionVelControlEnabled = false;
	private boolean rotationVelControlEnabled= false;
	
	private boolean isOutRange=false;
	private boolean insertionFinished=false;
	private JTextField txtX;
	private JTextField txtY;
	private JTextField txtZ;
	private JTextField txtInsertion;
	private JTextField txtRotation;
	
	public int calibrateX;
	public int calibrateY;
	public int calibrateZ;
	public int calibrateInsertion;
	public int calibrateRotation;
	private JTextField textFieldJogStep;
	
	
	// private JTable tableBaseinZFrameCalibration;
	// public boolean ;

	public ImageControlPanel(MainGUI kinematicsGUI) {
		setKinematicsGUI(kinematicsGUI);
		lblCartesianError = new JLabel("Stage X Axis");
		lblCartesianError.setBounds(new Rectangle(638, 304, 110, 21));
		lblCartesianError.setForeground(Color.BLUE);
		lblCartesianError.setText("Needle RAS Error");
		lblCartesianError.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblPosZ = new JLabel("Stage X Axis");
		lblPosZ.setBounds(new Rectangle(602, 390, 39, 23));
		lblPosZ.setForeground(Color.BLUE);
		lblPosZ.setText("PosZ");
		lblPosZ.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblPosY = new JLabel("Stage X Axis");
		lblPosY.setBounds(new Rectangle(602, 358, 37, 22));
		lblPosY.setForeground(Color.BLUE);
		lblPosY.setText("PosY");
		lblPosY.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblPosX = new JLabel("Stage X Axis");
		lblPosX.setBounds(new Rectangle(602, 332, 39, 24));
		lblPosX.setForeground(Color.BLUE);
		lblPosX.setText("PosX");
		lblPosX.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblYaw = new JLabel("Stage X Axis");
		lblYaw.setBounds(new Rectangle(602, 484, 42, 24));
		lblYaw.setForeground(Color.BLUE);
		lblYaw.setText("Yaw");
		lblYaw.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblPitch = new JLabel("Stage X Axis");
		lblPitch.setBounds(new Rectangle(602, 453, 45, 21));
		lblPitch.setForeground(Color.BLUE);
		lblPitch.setText("Pitch");
		lblPitch.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblRoll = new JLabel("Stage X Axis");
		lblRoll.setBounds(new Rectangle(602, 419, 40, 20));
		lblRoll.setForeground(Color.BLUE);
		lblRoll.setText("Roll");
		lblRoll.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblJointError = new JLabel("Stage X Axis");
		lblJointError.setBounds(new Rectangle(514, 305, 62, 22));
		lblJointError.setForeground(Color.BLUE);
		lblJointError.setText("Joint Error");
		lblJointError.setFont(new Font("Tahoma", Font.BOLD, 11));
		jLabel = new JLabel();
		jLabel.setBounds(new Rectangle(692, 214, 52, 30));
		jLabel.setText("50");

		setBorder(new BevelBorder(BevelBorder.LOWERED, null, null, null, null));
		setBackground(SystemColor.control);
		setLayout(null);
		setName("");
		add(jLabel, null);
		add(getJSlider(), null);

		double[][] tableZFrameRegistrationElements = { { 1, 0, 0, 0 },
				{ 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
				
		int[] boundsZFrameRegistration = { 10, 94, 200, 62 };
		tableZFrameRegistration = new EventResponseTable(
				tableZFrameRegistrationElements, boundsZFrameRegistration);
		tableZFrameRegistration.setBounds(218, 64, 170, 62);
		this.add(tableZFrameRegistration);

		double[][] tableBaseinZFrameCalibrationElements = { { 1, 0, 0, 0 },
				{ 0, 1, 0, 0}, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
		
		if (OpenIGTPanel.networkControlEnabled == true){
			tableBaseinZFrameCalibrationElements = new double[][] { { 1, 0, 0, 0 },
					{ 0, 1, 0, -33.55}, { 0, 0, 1, -219.24 }, { 0, 0, 0, 1 } };
		} 
		
		int[] baseinZFrameCalibrationVector = { 398, 64, 200, 62 };
		tableBaseinZFrameCalibration = new EventResponseTable(
				tableBaseinZFrameCalibrationElements,
				baseinZFrameCalibrationVector);
		tableBaseinZFrameCalibration.setLocation(395, 64);

		this.add(tableBaseinZFrameCalibration);

		textFieldXAxisDesiredJointPosition = new JTextField();
		textFieldXAxisDesiredJointPosition.setText("0");
		textFieldXAxisDesiredJointPosition.setBounds(193, 334, 90, 20);
		textFieldXAxisDesiredJointPosition.setColumns(10);

		textFieldYAxisDesiredJointPosition = new JTextField();
		textFieldYAxisDesiredJointPosition.setText("0");
		textFieldYAxisDesiredJointPosition.setBounds(193, 359, 90, 20);
		textFieldYAxisDesiredJointPosition.setColumns(10);

		JLabel lblStageYAxis = new JLabel("Stage Y Axis");
		lblStageYAxis.setForeground(Color.BLUE);
		lblStageYAxis.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblStageYAxis.setBounds(91, 357, 80, 25);
		this.add(lblStageYAxis);

		JLabel lblStageXAxis = new JLabel("Stage X Axis");
		lblStageXAxis.setForeground(Color.BLUE);
		lblStageXAxis.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblStageXAxis.setBounds(91, 332, 80, 25);
		this.add(lblStageXAxis);

		JLabel lblStageZAxis = new JLabel("Stage Z Axis");
		lblStageZAxis.setForeground(Color.BLUE);
		lblStageZAxis.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblStageZAxis.setBounds(91, 386, 80, 30);
		this.add(lblStageZAxis);

		JLabel lblDesiredJointPosition = new JLabel("Encoder Count");
		lblDesiredJointPosition.setForeground(Color.BLUE);
		lblDesiredJointPosition.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblDesiredJointPosition.setText("Desired Joint Pos");
		lblDesiredJointPosition.setBounds(191, 304, 122, 25);

		JLabel lblJointPosition = new JLabel("EncoderPosition");
		lblJointPosition.setForeground(Color.BLUE);
		lblJointPosition.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblJointPosition.setText("Joint Pos");
		lblJointPosition.setBounds(424, 306, 60, 20);

		JLabel lblInsertion = new JLabel("Insertion Axis");
		lblInsertion.setForeground(Color.BLUE);
		lblInsertion.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblInsertion.setBounds(91, 419, 82, 20);
		this.add(lblInsertion);

		JLabel lblRotationAxis = new JLabel("Rotation Axis");
		lblRotationAxis.setForeground(Color.BLUE);
		lblRotationAxis.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblRotationAxis.setBounds(91, 451, 80, 20);
		this.add(lblRotationAxis);

		JLabel lblRetractionAxis = new JLabel("Retraction Axis");
		lblRetractionAxis.setForeground(Color.BLUE);
		lblRetractionAxis.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblRetractionAxis.setBounds(91, 483, 100, 20);
		this.add(lblRetractionAxis);

		textFieldZAxisDesiredJointPosition = new JTextField();
		textFieldZAxisDesiredJointPosition.setText("0");
		textFieldZAxisDesiredJointPosition.setBounds(193, 391, 90, 20);
		this.add(lblDesiredJointPosition, lblDesiredJointPosition.getName());
		this.add(textFieldXAxisDesiredJointPosition,
				textFieldXAxisDesiredJointPosition.getName());
		this.add(textFieldYAxisDesiredJointPosition,
				textFieldYAxisDesiredJointPosition.getName());
		this.add(textFieldZAxisDesiredJointPosition);
		textFieldZAxisDesiredJointPosition.setColumns(10);

		textFieldZAxisJointPosition = new JTextField();
		textFieldZAxisJointPosition.setEditable(false);
		textFieldZAxisJointPosition.setText("0");
		textFieldZAxisJointPosition.setBounds(426, 391, 55, 20);
		textFieldZAxisJointPosition.setColumns(10);

		textFieldInsertionDesiredJointPosition = new JTextField();
		textFieldInsertionDesiredJointPosition.setText("0");
		textFieldInsertionDesiredJointPosition.setColumns(10);
		textFieldInsertionDesiredJointPosition.setBounds(193, 419, 90, 20);
		this.add(textFieldInsertionDesiredJointPosition);

		textFieldRotationDesiredJointPosition = new JTextField();
		textFieldRotationDesiredJointPosition.setText("0");
		textFieldRotationDesiredJointPosition.setColumns(10);
		textFieldRotationDesiredJointPosition.setBounds(193, 451, 90, 20);
		this.add(textFieldRotationDesiredJointPosition);

		textFieldInsertionJointPosition = new JTextField();
		textFieldInsertionJointPosition.setText("0");
		textFieldInsertionJointPosition.setEditable(false);
		textFieldInsertionJointPosition.setColumns(10);
		textFieldInsertionJointPosition.setBounds(426, 419, 55, 20);
		this.add(lblJointPosition, lblJointPosition.getName());
		this.add(getTextFieldXAxisJointPosition(), null);
		this.add(getTextFieldYAxisJointPosition(), null);
		this.add(textFieldZAxisJointPosition,
				textFieldZAxisJointPosition.getName());
		this.add(textFieldInsertionJointPosition);

		textFieldRotationJointPosition = new JTextField();
		textFieldRotationJointPosition.setText("0");
		textFieldRotationJointPosition.setEditable(false);
		textFieldRotationJointPosition.setColumns(10);
		textFieldRotationJointPosition.setBounds(426, 451, 55, 20);
		this.add(textFieldRotationJointPosition);

		textFieldRetractionDesiredJointPosition = new JTextField();
		textFieldRetractionDesiredJointPosition.setText("0");
		textFieldRetractionDesiredJointPosition.setColumns(10);
		textFieldRetractionDesiredJointPosition.setBounds(193, 483, 90, 20);

		textFieldRetractionJointPosition = new JTextField();
		textFieldRetractionJointPosition.setText("0");
		textFieldRetractionJointPosition.setEditable(false);
		textFieldRetractionJointPosition.setColumns(10);
		textFieldRetractionJointPosition.setBounds(426, 483, 55, 20);
		this.add(textFieldRetractionDesiredJointPosition,
				textFieldRetractionDesiredJointPosition.getName());
		this.add(textFieldRetractionJointPosition);

		separator = new JSeparator();
		separator.setBackground(Color.WHITE);
		separator.setForeground(Color.BLACK);
		separator.setBounds(10, 37, 794, 2);
		this.add(lblJointError, null);
		this.add(getTextFieldXAxisJointError(), null);
		this.add(getTextFieldYAxisJointError(), null);
		this.add(getTextFieldZAxisJointError(), null);
		this.add(getTextFieldInsertionJointError(), null);
		this.add(getTextFieldRotationJointError(), null);

		separator_1 = new JSeparator();
		separator_1.setForeground(Color.BLACK);
		separator_1.setBackground(Color.WHITE);
		separator_1.setBounds(13, 292, 791, 20);
		this.add(getTextFieldRetractionJointError(), null);

		lblZFrameRegistrition = new JLabel("Z Frame Registration");
		lblZFrameRegistrition.setForeground(Color.BLUE);
		lblZFrameRegistrition.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblZFrameRegistrition.setBounds(225, 40, 157, 25);
	
		double[][] tableDesiredNeedleRASElements = { { 1, 0, 0, 12.5 },
				{ 0, 1, 0, 156}, { 0, 0, 1, 275}, { 0, 0, 0, 1 } };
		
		if (OpenIGTPanel.networkControlEnabled == true){
			 tableDesiredNeedleRASElements = new double[][] { { 1, 0, 0, 12.5 },
					{ 0, 1, 0, 135 }, { 0, 0, 1, 100 }, { 0, 0, 0, 1 } };
		} 
		
		int[] boundsTableDesiredNeedleRAS = { 220, 94, 200, 62 };
		tableDesiredNeedleRAS = new EventResponseTable(
				tableDesiredNeedleRASElements, boundsTableDesiredNeedleRAS);
		tableDesiredNeedleRAS.setBounds(22, 64, 170, 62);

		lblDesiredNeedleRas = new JLabel("Desired Needle Pos");
		lblDesiredNeedleRas.setForeground(Color.BLUE);
		lblDesiredNeedleRas.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblDesiredNeedleRas.setBounds(24, 40, 167, 25);
		this.add(lblDesiredNeedleRas, lblDesiredNeedleRas.getName());
		this.add(tableDesiredNeedleRAS, tableDesiredNeedleRAS.getName());
		this.add(lblZFrameRegistrition, lblZFrameRegistrition.getName());

		tableActualNeedleRAS = new JTable();
		tableActualNeedleRAS.setModel(new DefaultTableModel(new Object[][] {
				{ new Double(1.0), new Double(0.0), new Double(0.0),
						new Double(0.1) },
				{ new Double(0.0), new Double(1.0), new Double(0.0),
						new Double(0.1) },
				{ new Double(0.0), new Double(0.0), new Double(1.0),
						new Double(0.1) },
				{ new Double(0.0), new Double(0.0), new Double(0.0),
						new Double(1.0) }, }, new String[] { "s", "t", "r",
				"Position" }) {
			
			private static final long serialVersionUID = 5881532877939470175L;
			@SuppressWarnings("rawtypes")
			Class[] columnTypes = new Class[] { Double.class, Double.class,
					Double.class, Double.class };

			@SuppressWarnings({ "unchecked", "rawtypes" })
			public Class getColumnClass(int columnIndex) {
				return columnTypes[columnIndex];
			}
		});
		tableActualNeedleRAS.getColumnModel().getColumn(0).setResizable(false);
		tableActualNeedleRAS.getColumnModel().getColumn(0).setMaxWidth(100);
		tableActualNeedleRAS.getColumnModel().getColumn(1).setResizable(false);
		tableActualNeedleRAS.getColumnModel().getColumn(1).setMaxWidth(100);
		tableActualNeedleRAS.getColumnModel().getColumn(2).setResizable(false);
		tableActualNeedleRAS.getColumnModel().getColumn(2).setMaxWidth(100);
		tableActualNeedleRAS.getColumnModel().getColumn(3).setResizable(false);
		tableActualNeedleRAS.getColumnModel().getColumn(3).setMaxWidth(100);
		tableActualNeedleRAS.setBounds(604, 64, 200, 60);
		tableActualNeedleRAS.setEnabled(false);

		lblActualNeedleRas = new JLabel("Actual Needle RAS");
		lblActualNeedleRas.setForeground(Color.BLUE);
		lblActualNeedleRas.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblActualNeedleRas.setBounds(471, 68, 167, 25);

		lblActualNeedleRas_1 = new JLabel("Actual Needle");
		lblActualNeedleRas_1.setForeground(Color.BLUE);
		lblActualNeedleRas_1.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblActualNeedleRas_1.setBounds(638, 40, 122, 25);
		this.add(lblActualNeedleRas_1, lblActualNeedleRas_1.getName());
		this.add(tableActualNeedleRAS, tableActualNeedleRAS.getName());

		this.add(separator_1, separator_1.getName());
		this.add(lblCartesianError, null);
		this.add(lblPosX, null);
		this.add(lblPosY, null);
		this.add(lblPosZ, null);
		this.add(lblRoll, null);
		this.add(lblPitch, null);
		this.add(lblYaw, null);
		this.add(getTextFieldXAxisNeedleCartesianError(), null);
		this.add(getTextFieldYAxisNeedleCartesianError(), null);
		this.add(getTextFieldZAxisNeedleCartesianError(), null);
		this.add(getTextFieldRollNeedleCartesianError(), null);
		this.add(getTextFieldXPitchNeedleCartesianError(), null);
		this.add(getTextFieldYawNeedleCartesianError(), null);
		this.add(getJButtonStop(), null);
		this.add(getJButtonInsertNeedle(), null);
		this.add(getJButtonCalculcateInvKinematics(), null);
		this.add(getJButtonRetractNeedle(), null);
		this.add(getJButtonRetractStylet(), null);
		this.add(getJButtonHomePosition(), null);
		this.add(separator, separator.getName());
		this.add(getJButtonZFrameRegistration(), null);
		this.add(getjButtonDesiredNeedleRAS(), null);

		final JButton jButtonRobotMove = new JButton("Move to Entry Point");
		jButtonRobotMove.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {

				if (isEnableRobotMove() == false) {
					setEnableRobotMove(true);
					jButtonRobotMove.setText("Robot moved");
					
					desiredJointTicksInteger[0] = (int) Math.floor(desiredJointTicksDouble[0]);
					desiredJointTicksInteger[1] = (int) Math.floor(desiredJointTicksDouble[1]);
					// only update insertion axis
					desiredJointTicksInteger[2] = 0;	
					desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[2];	
					desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[3];	

					//TODO parameterize the time					
					runDesiredJointUpdate(2);
					
				} else {
					setEnableRobotMove(false);
					jButtonRobotMove.setText("Move Robot");
				}
			}
		});
		jButtonRobotMove.setBounds(60, 568, 220, 25);
		add(jButtonRobotMove);

		JLabel lblNeedleTranslation = new JLabel("Needle Translation");
		lblNeedleTranslation.setForeground(Color.BLUE);
		lblNeedleTranslation.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblNeedleTranslation.setBounds(171, 166, 142, 25);
		add(lblNeedleTranslation);

		JLabel lblNeedleRotation = new JLabel("Needle Rotation");
		lblNeedleRotation.setForeground(Color.BLUE);
		lblNeedleRotation.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblNeedleRotation.setBounds(374, 166, 128, 25);
		add(lblNeedleRotation);

		final JLabel lblInsertionVelocity = new JLabel("Velocity(mm/s)");
		lblInsertionVelocity.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblInsertionVelocity.setForeground(Color.BLUE);
		lblInsertionVelocity.setBounds(163, 194, 100, 16);
		add(lblInsertionVelocity);

		JLabel lblNeedleInsertionTime = new JLabel("Time(ms)");
		lblNeedleInsertionTime.setForeground(Color.BLUE);
		lblNeedleInsertionTime.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblNeedleInsertionTime.setBounds(163, 221, 90, 16);
		add(lblNeedleInsertionTime);

		JSeparator separator_2 = new JSeparator();
		separator_2.setForeground(Color.BLACK);
		separator_2.setBackground(Color.WHITE);
		separator_2.setBounds(13, 163, 791, 20);
		add(separator_2);

		textFieldInsertionVelocity = new JTextField();
		textFieldInsertionVelocity.setText("0");
		textFieldInsertionVelocity.setBounds(259, 192, 50, 20);
		add(textFieldInsertionVelocity);
		textFieldInsertionVelocity.setColumns(10);

		textFieldInsertionAcceleration = new JTextField();
		textFieldInsertionAcceleration.setText("50");
		textFieldInsertionAcceleration.setBounds(259, 219, 50, 20);
		add(textFieldInsertionAcceleration);
		textFieldInsertionAcceleration.setColumns(10);

		final JButton btnInsertionConfirm = new JButton("Enable Insertion Control");
		btnInsertionConfirm.setBounds(141, 263, 200, 24);
		add(btnInsertionConfirm);
		btnInsertionConfirm.addActionListener(new ActionListener() {
			boolean enableButton = true;

			public void actionPerformed(ActionEvent e) {
				enableButton = !enableButton;

				// test the velocity command format
				try{
					setInsertionVel(Double.parseDouble(textFieldInsertionVelocity.getText()));	
				}
				catch(NumberFormatException ex){
					JOptionPane.showMessageDialog(new JFrame(), "Insertion velocity must be number", "Error",
				            JOptionPane.ERROR_MESSAGE);
					textFieldInsertionVelocity.setText("0");
				}
				
				// test the acceleration command format
				try{
					setDesiredInsertionAccel(Double.parseDouble(textFieldInsertionAcceleration.getText()));
				}
				catch(NumberFormatException ex){
					JOptionPane.showMessageDialog(new JFrame(), "Insertion acceleration must be number", "Error",
				            JOptionPane.ERROR_MESSAGE);
					textFieldInsertionAcceleration.setText("0");
				}																
					setInsertionVelControlEnabled(!enableButton);
					
					System.out.println("isInsertionVelControlEnabled "+isInsertionVelControlEnabled());	
					textFieldInsertionVelocity.setEditable(enableButton);
					textFieldInsertionAcceleration.setEditable(enableButton);

				if (isInsertionVelControlEnabled()==false){
					btnInsertionConfirm.setText("Enable Insertion Control");
				}
				else{
					btnInsertionConfirm.setText("Disable Insertion Control");
				}
			}
		});

		textFieldNeedleRotation = new JTextField();
		textFieldNeedleRotation.setText("0");
		textFieldNeedleRotation.setBounds(483, 192, 50, 20);
		add(textFieldNeedleRotation);
		textFieldNeedleRotation.setColumns(10);

		textFieldNeedleRotationVelocity = new JTextField();
		textFieldNeedleRotationVelocity.setText("50");
		textFieldNeedleRotationVelocity.setBounds(483, 219, 50, 20);
		add(textFieldNeedleRotationVelocity);
		textFieldNeedleRotationVelocity.setColumns(10);

		final JRadioButton rdbtnNeedleClockwise = new JRadioButton("Clockwise");
		rdbtnNeedleClockwise.setSelected(true);
		rdbtnNeedleClockwise.setBounds(379, 239, 90, 24);
		add(rdbtnNeedleClockwise);

		final JRadioButton rdbtnNeedleCounterClockwise = new JRadioButton(
				"Counterclockwise");
		rdbtnNeedleCounterClockwise.setBounds(467, 239, 142, 24);
		add(rdbtnNeedleCounterClockwise);

		rdbtnNeedleClockwise.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				clockwiseEnabled = true;
				// Cannot refer to a non-final variable
				// rdbtnNeedleCounterClockwise
				// inside an inner class defined in a different method
				// rdbtnNeedleCounterClockwise.setEnabled(!clockwiseEnabled);
				rdbtnNeedleCounterClockwise.setSelected(!clockwiseEnabled);
			}
		});

		rdbtnNeedleCounterClockwise.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				clockwiseEnabled = false;
				// Cannot refer to a non-final variable
				// rdbtnNeedleCounterClockwise
				// inside an inner class defined in a different method
				rdbtnNeedleClockwise.setSelected(clockwiseEnabled);
			}
		});

		JLabel lblNeedleRotationdeg = new JLabel("Rotation(deg)");
		lblNeedleRotationdeg.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblNeedleRotationdeg.setForeground(Color.BLUE);
		lblNeedleRotationdeg.setBounds(379, 194, 90, 16);
		add(lblNeedleRotationdeg);

		JLabel lblNeedleRotationTime = new JLabel("Time (ms)");
		lblNeedleRotationTime.setForeground(Color.BLUE);
		lblNeedleRotationTime.setFont(new Font("Tahoma", Font.BOLD, 11));
		lblNeedleRotationTime.setBounds(378, 221, 90, 16);
		add(lblNeedleRotationTime);

		JButton btnRotationConfirm = new JButton("Enable Rotaion Control");
		btnRotationConfirm.setBounds(374, 263, 200, 24);
		add(btnRotationConfirm);

		btnRotationConfirm.addActionListener(new ActionListener() {
			boolean enableButton = true;

			public void actionPerformed(ActionEvent e) {
				setRotationVelControlEnabled(true);
				
				enableButton = !enableButton;

				try{
					setDesiredRotationAngle(Double.parseDouble(textFieldNeedleRotation.getText()));
				}
				catch(NumberFormatException ex){
					JOptionPane.showMessageDialog(new JFrame(), "Rotation angle must be number", "Error",
				            JOptionPane.ERROR_MESSAGE);
					textFieldNeedleRotation.setText("0");
				}
				
				try{
					setDesiredRotationVel(Double.parseDouble(textFieldNeedleRotationVelocity.getText()));
				}
				catch(NumberFormatException ex){
					JOptionPane.showMessageDialog(new JFrame(), "Rotation velocity must be number", "Error",
				            JOptionPane.ERROR_MESSAGE);
					textFieldNeedleRotationVelocity.setText("0");
				}
										
					rotationVelControlEnabled=!enableButton;
					textFieldNeedleRotation.setEditable(enableButton);
					textFieldNeedleRotationVelocity.setEditable(enableButton);
				
				if (isRotationVelControlEnabled()==false){
					btnInsertionConfirm.setText("Enable Rotation Control");
				}
				else{
					btnInsertionConfirm.setText("Disable Rotation Control");
				}
				
			}
		});

		JLabel lblBaseInZFrameCalibration = new JLabel("Base in Z Calibration");
		lblBaseInZFrameCalibration.setForeground(Color.BLUE);
		lblBaseInZFrameCalibration.setFont(new Font("Tahoma", Font.BOLD, 15));
		lblBaseInZFrameCalibration.setBounds(417, 40, 157, 25);
		add(lblBaseInZFrameCalibration);

		jButtonBaseinZFrameCalibration = new JButton("Confirm Calibration");
		jButtonBaseinZFrameCalibration.addActionListener(new ActionListener() {
			boolean enableTable = true;
			public void actionPerformed(ActionEvent e) {				
				if (enableTable==true){
					jButtonBaseinZFrameCalibration.setText("Calibration Confirmed");
				}
				else
				{
					jButtonBaseinZFrameCalibration.setText("Confirm Calibration");
				}				
				enableTable = !enableTable;
				tableBaseinZFrameCalibration.setEnabled(enableTable);
			}
		});
		jButtonBaseinZFrameCalibration.setBounds(405, 131, 180, 23);
		add(jButtonBaseinZFrameCalibration);

		// checkbox for MRI test
		final JCheckBox checkBoxMRIImageControl = new JCheckBox("MRI Test");
		checkBoxMRIImageControl.setSelected(true);
		checkBoxMRIImageControl.setBounds(6, 7, 80, 23);
		add(checkBoxMRIImageControl);

		final JCheckBox checkBoxPhysicalControl = new JCheckBox(
				"Physical Control");
		checkBoxPhysicalControl.setBounds(88, 7, 110, 23);
		add(checkBoxPhysicalControl);

		final JButton btnStartRotation = new JButton("Start Rotation");
		btnStartRotation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				if (isEnableRotation() == false) {
					setEnableRotation(true);
					btnStartRotation.setText("Stop Rotation");
					
					desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
					desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
					desiredJointTicksInteger[2] = 0;
					desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[2];
					// only update rotation axis
					desiredJointTicksInteger[4] = (int) Math.floor(desiredJointTicksDouble[3]);	
					
					
					int velocity = 100;//ticks per second
					double seconds = 5;//seconds
					runDesiredJointUpdate(seconds);
					runNeedleRotationVelocity( velocity,  seconds ) ;
					try {
						Thread.sleep((long)(seconds*1000));
					} catch (InterruptedException e1) {
						// TODO Auto-generated catch block
						e1.printStackTrace();
					}										
					
				} else {
					desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
					desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
					desiredJointTicksInteger[2] = 0;
					desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[2];
					desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[3];
					runDesiredJointUpdate(0.01);
					
					setEnableRotation(false);
					btnStartRotation.setText("Start Rotation");
				}
			}
		});
		btnStartRotation.setBounds(290, 532, 135, 25);
		add(btnStartRotation);
		
		txtX = new JTextField();
		txtX.setText("0");
		txtX.setBounds(304, 7, 40, 20);
		add(txtX);
		txtX.setColumns(10);		
		txtX.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				calibrateX = Integer.parseInt(txtX.getText());	
				System.out.println(calibrateX);
			}
		});	
			
		txtY = new JTextField();
		txtY.setText("0");
		txtY.setColumns(10);
		txtY.setBounds(376, 7, 40, 20);
		add(txtY);		
		txtY.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				calibrateY = Integer.parseInt(txtY.getText());	
				System.out.println(calibrateY);
			}
		});	
				
		txtZ = new JTextField();
		txtZ.setText("0");
		txtZ.setColumns(10);
		txtZ.setBounds(451, 7, 40, 20);
		add(txtZ);
		txtZ.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				calibrateZ = Integer.parseInt(txtZ.getText());	
				System.out.println(calibrateZ);
			}
		});	
		
		txtInsertion = new JTextField();
		txtInsertion.setText("0");
		txtInsertion.setColumns(10);
		txtInsertion.setBounds(559, 8, 40, 20);
		add(txtInsertion);
		txtInsertion.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				calibrateInsertion = Integer.parseInt(txtInsertion.getText());	
				System.out.println(calibrateInsertion);
			}
		});	
		
		txtRotation = new JTextField();
		txtRotation.setText("0");
		txtRotation.setColumns(10);
		txtRotation.setBounds(671, 8, 40, 20);
		add(txtRotation);
		txtRotation.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				calibrateRotation = Integer.parseInt(txtInsertion.getText());	
				System.out.println(calibrateRotation);
			}
		});	
		
		JLabel lblEncoderZeros = new JLabel("Encoder Zeros:");
		lblEncoderZeros.setBounds(204, 10, 80, 14);
		add(lblEncoderZeros);
		
		
		
		JLabel lblX = new JLabel("X:");
		lblX.setBounds(283, 10, 20, 14);
		add(lblX);
		
		JLabel lblY = new JLabel("Y:");
		lblY.setBounds(360, 10, 20, 14);
		add(lblY);
		
		JLabel lblZ = new JLabel("Z:");
		lblZ.setBounds(441, 10, 20, 14);
		add(lblZ);
		
		JLabel lblInsert = new JLabel("Insertion:");
		lblInsert.setBounds(501, 11, 50, 14);
		add(lblInsert);
		
		JLabel lblRotation = new JLabel("Rotation:");
		lblRotation.setBounds(609, 11, 52, 14);
		add(lblRotation);
		
		JButton btnConfirmCalibration = new JButton("Confirm Zero");
		btnConfirmCalibration.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent act) {				
				System.out.println(calibrateX);
				System.out.println(calibrateY);
				System.out.println(calibrateZ);
				System.out.println(calibrateInsertion);
				System.out.println(calibrateRotation);
			}
		});
		btnConfirmCalibration.setBounds(725, 6, 135, 23);
		add(btnConfirmCalibration);
		
		JButton buttonResetY = new JButton("Reset");
		buttonResetY.setBounds(6, 359, 80, 20);
		add(buttonResetY);
		
		JButton buttonResetX = new JButton("Reset");
		buttonResetX.setBounds(6, 334, 80, 20);
		add(buttonResetX);
		
		JButton buttonResetZ = new JButton("Reset");
		buttonResetZ.setBounds(6, 391, 80, 20);
		add(buttonResetZ);
		
		JButton buttonResetInsertion = new JButton("Reset");
		buttonResetInsertion.setBounds(6, 419, 80, 20);
		add(buttonResetInsertion);
		
		JButton buttonResetRotation = new JButton("Reset");
		buttonResetRotation.setBounds(6, 450, 80, 20);
		add(buttonResetRotation);
		
		JButton buttonResetRetraction = new JButton("Reset");
		buttonResetRetraction.setBounds(6, 482, 80, 20);
		add(buttonResetRetraction);
		
		JButton buttonJogPlusX = new JButton("Jog+");
		buttonJogPlusX.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				
				double jogStep = Double.parseDouble(textFieldJogStep.getText());
				double desiredJointPosition = Double.parseDouble(textFieldXAxisDesiredJointPosition.getText());
				textFieldXAxisDesiredJointPosition.setText(Double.toString(desiredJointPosition+jogStep));
				
			}
		});		
		buttonJogPlusX.setBounds(287, 334, 57, 20);
		add(buttonJogPlusX);
		
		JButton buttonJogMinusX = new JButton("Jog-");
		buttonJogMinusX.setBounds(351, 334, 57, 20);
		add(buttonJogMinusX);
		
		JButton buttonJogPlusY = new JButton("Jog+");
		buttonJogPlusY.setBounds(287, 359, 57, 20);
		add(buttonJogPlusY);
		
		JButton buttonJogMinusY = new JButton("Jog-");
		buttonJogMinusY.setBounds(351, 359, 57, 20);
		add(buttonJogMinusY);
		
		JButton buttonJogPlusZ = new JButton("Jog+");
		buttonJogPlusZ.setBounds(287, 391, 57, 20);
		add(buttonJogPlusZ);
		
		JButton buttonJogPlusInsertion = new JButton("Jog+");
		buttonJogPlusInsertion.setBounds(287, 418, 57, 20);
		add(buttonJogPlusInsertion);
		
		JButton buttonJogPlusRotation = new JButton("Jog+");
		buttonJogPlusRotation.setBounds(287, 450, 57, 20);
		add(buttonJogPlusRotation);
		
		JButton buttonJogPlusRetraction = new JButton("Jog+");
		buttonJogPlusRetraction.setBounds(287, 482, 57, 20);
		add(buttonJogPlusRetraction);
		
		JButton buttonJogMinusZ = new JButton("Jog-");
		buttonJogMinusZ.setBounds(351, 391, 57, 20);
		add(buttonJogMinusZ);
		
		JButton buttonJogMinusInsertion = new JButton("Jog-");
		buttonJogMinusInsertion.setBounds(351, 418, 57, 20);
		add(buttonJogMinusInsertion);
		
		JButton buttonJogMinusRotation = new JButton("Jog-");
		buttonJogMinusRotation.setBounds(351, 450, 57, 20);
		add(buttonJogMinusRotation);
		
		JButton buttonJogMinusRetraction = new JButton("Jog-");
		buttonJogMinusRetraction.setBounds(351, 482, 57, 20);
		add(buttonJogMinusRetraction);
		
		textFieldJogStep = new JTextField();
		textFieldJogStep.setText("1");
		textFieldJogStep.setBounds(323, 306, 30, 20);
		add(textFieldJogStep);
		textFieldJogStep.setColumns(10);
		
		JButton btnEnableJogging = new JButton("Enable jogging");
		btnEnableJogging.setBounds(60, 604, 220, 23);
		add(btnEnableJogging);
		
		JButton btnMoveAllJoints = new JButton("Move All Joints");
		btnMoveAllJoints.setBounds(290, 604, 265, 23);
		add(btnMoveAllJoints);
		
		JLabel lblJogStepUnit = new JLabel("mm/deg");
		lblJogStepUnit.setBounds(351, 309, 46, 14);
		add(lblJogStepUnit);

		// action listener
		checkBoxMRIImageControl.addActionListener(new ActionListener() {
			boolean enableCheckBox = true;

			public void actionPerformed(ActionEvent e) {
				// enableCheckBox=!enableCheckBox;
				checkBoxMRIImageControl.setSelected(enableCheckBox);
				// tableZFrameRegistration.setEnabled(enableCheckBox);
				checkBoxPhysicalControl.setSelected(!enableCheckBox);
				jButtonBaseinZFrameCalibration.setEnabled(enableCheckBox);
				jButtonZFrameRegistration.setEnabled(enableCheckBox);
			}
		});

		// action listener
		checkBoxPhysicalControl.addActionListener(new ActionListener() {
			boolean enableCheckBox = true;

			public void actionPerformed(ActionEvent e) {
				// enableCheckBox=!enableCheckBox;
				checkBoxPhysicalControl.setSelected(enableCheckBox);
				// tableZFrameRegistration.setEnabled(enableCheckBox);
				checkBoxMRIImageControl.setSelected(!enableCheckBox);
				tableZFrameRegistration.setEnabled(!enableCheckBox);
				tableBaseinZFrameCalibration.setEnabled(!enableCheckBox);
				jButtonBaseinZFrameCalibration.setEnabled(!enableCheckBox);
				jButtonZFrameRegistration.setEnabled(!enableCheckBox);
			}
		});
	}

	/**
	 * This method initializes jSlider
	 * 
	 * @return javax.swing.JSlider
	 */
	private JSlider getJSlider() {
		if (jSlider == null) {
			jSlider = new JSlider();
			jSlider.setBounds(new Rectangle(559, 184, 247, 20));
			jSlider.addChangeListener(new javax.swing.event.ChangeListener() {
				public void stateChanged(javax.swing.event.ChangeEvent e) {
					System.out.println("stateChanged()");
					int frequency = jSlider.getValue();
					jLabel.setText(Integer.toString(frequency));
				}
			});
		}
		return jSlider;
	}

	/**
	 * This method initializes jButtonCalculcateInvKinematics
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonCalculcateInvKinematics() {
		
		if (jButtonCalculcateInvKinematics == null) {
			jButtonCalculcateInvKinematics = new JButton();
			jButtonCalculcateInvKinematics.setText("Calculate Joint Position");
			jButtonCalculcateInvKinematics.setBounds(new Rectangle(60, 533, 220, 25));
						
			jButtonCalculcateInvKinematics.addActionListener(new ActionListener() {
				 boolean enableButton = true;
						public void actionPerformed(ActionEvent e) {
							if ((!tableZFrameRegistration.isEnabled())& (!tableDesiredNeedleRAS.isEnabled())&(!tableBaseinZFrameCalibration.isEnabled())) {
								
								if (enableButton ==true){
									jButtonCalculcateInvKinematics.setText("Joint Position Calculated");
								}
								else
								{
									jButtonCalculcateInvKinematics.setText("Calculate Joint Position");
								}					
								// every click would switch to enable/disable the table
								enableButton = !enableButton;
								// 1 step: calculate desired tip position (unit: mm)
								// 3x1 vector
								desiredCartesianSpaceVector = getRobotKinematics()
										.calculateDesiredTipPosition(
												tableDesiredNeedleRAS,
												tableZFrameRegistration,
												tableBaseinZFrameCalibration);
								
								System.out.println("++++++++++++++++++++");
								System.out
										.println("desiredCartesianSpaceVector 0 "
												+ desiredCartesianSpaceVector[0]);
								System.out
										.println("desiredCartesianSpaceVector 1 "
												+ desiredCartesianSpaceVector[1]);
								System.out
										.println("desiredCartesianSpaceVector 2 "
												+ desiredCartesianSpaceVector[2]);
								System.out.println("++++++++++++++++++++");
								
								// if it does not exceed workspace
								// only proceed when isside workspace
								if (getRobotKinematics().checkCartesianSpaceRange(desiredCartesianSpaceVector)==false){
									System.out.println("++++++++++++++++++++");
									
									// 2 step: calculate desired joint space (unit:
									// mm and degree)
									// 4x1 vector
									try {
										desiredJointSpaceVector = getRobotKinematics()
												.inverseKinematics(desiredCartesianSpaceVector);
									} catch (Exception excp) {
										setOutOfRange(true);
										excp.printStackTrace();
									}
									
									setOutOfRange(getRobotKinematics()
											.checkJointSpaceRange(desiredJointSpaceVector));								

									System.out.println("********************");
									System.out.println("\n desiredJointSpaceVector X axis  "
											+ desiredJointSpaceVector[0]);
									System.out.println("desiredJointSpaceVector Y axis  "
											+ desiredJointSpaceVector[1]);
									System.out.println("desiredJointSpaceVector Insertion axis "
											+ desiredJointSpaceVector[3]);
									System.out.println("desiredJointSpaceVector Rotation axis "
											+ desiredJointSpaceVector[4]);
									System.out.println("********************");

									// if it does not exceed joint workspace
									if (isOutRange()==false){										
										// 3 step: calculate encoder ticks
										// all the joint ticks are calculated and saved here, this is the real inverse kinematics ticks
										desiredJointTicksDouble = getRobotKinematics()
												.jointSIUnits2EncoderTicks(desiredJointSpaceVector);

										// convert to integer desiredJointTicksInteger
										// 4x1
										//X axis
										desiredJointTicksInteger[0] = (int) Math.floor(desiredJointTicksDouble[0]);
										//Y axis
										desiredJointTicksInteger[1] = (int) Math.floor(desiredJointTicksDouble[1]);		
										
										desiredJointTicksInteger[2] = 0;								
										//insertion
										desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[2];
										//rotation
										desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[3];
									}else {
										desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
										desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];		
										desiredJointTicksInteger[2] = 0;								
										desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[2];
										desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[3];
									}
																	
									System.out.println("++++++++++++++++++++");
									System.out.println("Desired Joint Ticks Double 0 "
											+ (int) Math
													.floor(desiredJointTicksDouble[0]));
									System.out.println("Desired Joint Ticks Double 1 "
											+ (int) Math
													.floor(desiredJointTicksDouble[1]));
									System.out.println("Desired Joint Ticks Double 2 "
											+ (int) Math
													.floor(desiredJointTicksDouble[2]));
									System.out.println("Desired Joint Ticks Double 3 "
											+ (int) Math
													.floor(desiredJointTicksDouble[3]));
									System.out.println("Desired Joint Ticks Double 4 "
											+ (int) Math
													.floor(desiredJointTicksDouble[4]));
									
									System.out.println("++++++++++++++++++++");

									// format to display 3 digits of double data
									DecimalFormat formatDisplay = new DecimalFormat();
									formatDisplay.setMaximumFractionDigits(3);

									// System.out.println(formatDisplay.format(jointSpaceVector[0]));
									// update text field with desired joint motion
									textFieldXAxisDesiredJointPosition.setText(formatDisplay
											.format(desiredJointSpaceVector[0]));
									textFieldYAxisDesiredJointPosition.setText(formatDisplay
											.format(desiredJointSpaceVector[1]));
									// TODO, find the display error of Y axis
									// Z axis of the base stage is not using, so it
									// is zero
									textFieldZAxisDesiredJointPosition
											.setText("No Z motion");
									textFieldInsertionDesiredJointPosition.setText(formatDisplay
											.format(desiredJointSpaceVector[3]));
									textFieldRotationDesiredJointPosition
											.setText("Continous");
									textFieldRetractionDesiredJointPosition
											.setText("No retraction");

									// ******************************
									textFieldXAxisJointError.setText(formatDisplay
											.format(desiredJointSpaceVector[0] - getKinematicsGUI().getCurrentJointTicksValues()[0]));
									textFieldYAxisJointError.setText(formatDisplay
											.format(desiredJointSpaceVector[1] - getKinematicsGUI().getCurrentJointTicksValues()[1]));
									textFieldInsertionJointError.setText(formatDisplay
											.format(desiredJointSpaceVector[3] - getKinematicsGUI().getCurrentJointTicksValues()[3]));
									textFieldZAxisJointError.setText("NA");
									textFieldRotationJointError.setText(formatDisplay
											.format(desiredJointSpaceVector[4] - getKinematicsGUI().getCurrentJointTicksValues()[4]));
									textFieldRetractionJointError.setText("NA");									
								}
								// ***********************************************************************************************								
							}
						}
					});

		}
		return jButtonCalculcateInvKinematics;
	}

	// TODO
	// the original is private type function
	public JTextField getTextFieldXAxisJointPosition() {
		if (textFieldXAxisJointPosition == null) {
			textFieldXAxisJointPosition = new JTextField();
			textFieldXAxisJointPosition.setEditable(false);
			textFieldXAxisJointPosition.setBounds(new Rectangle(426, 334, 55, 20));
			textFieldXAxisJointPosition.setColumns(10);
			textFieldXAxisJointPosition.setText("0");
		}
		return textFieldXAxisJointPosition;
	}

	/**
	 * This method initializes textFieldYAxisJointPosition
	 * 
	 * @return javax.swing.JTextField
	 */
	public JTextField getTextFieldYAxisJointPosition() {
		if (textFieldYAxisJointPosition == null) {
			textFieldYAxisJointPosition = new JTextField();
			textFieldYAxisJointPosition.setEditable(false);
			textFieldYAxisJointPosition.setBounds(new Rectangle(426, 359, 55, 20));
			textFieldYAxisJointPosition.setColumns(10);
			textFieldYAxisJointPosition.setText("0");
		}
		return textFieldYAxisJointPosition;
	}

	/**
	 * This method initializes textFieldXAxisJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldXAxisJointError() {
		if (textFieldXAxisJointError == null) {
			textFieldXAxisJointError = new JTextField();
			textFieldXAxisJointError.setBounds(new Rectangle(510, 334, 70, 20));
			textFieldXAxisJointError.setEditable(false);
			textFieldXAxisJointError.setColumns(10);
			textFieldXAxisJointError.setText("0");
		}
		return textFieldXAxisJointError;
	}

	/**
	 * This method initializes textFieldYAxisJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldYAxisJointError() {
		if (textFieldYAxisJointError == null) {
			textFieldYAxisJointError = new JTextField();
			textFieldYAxisJointError.setBounds(new Rectangle(510, 359, 70, 20));
			textFieldYAxisJointError.setEditable(false);
			textFieldYAxisJointError.setColumns(10);
			textFieldYAxisJointError.setText("0");
		}
		return textFieldYAxisJointError;
	}

	/**
	 * This method initializes textFieldZAxisJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldZAxisJointError() {
		if (textFieldZAxisJointError == null) {
			textFieldZAxisJointError = new JTextField();
			textFieldZAxisJointError.setBounds(new Rectangle(510, 391, 70, 20));
			textFieldZAxisJointError.setEditable(false);
			textFieldZAxisJointError.setColumns(10);
			textFieldZAxisJointError.setText("0");
		}
		return textFieldZAxisJointError;
	}

	/**
	 * This method initializes textFieldInsertionJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldInsertionJointError() {
		if (textFieldInsertionJointError == null) {
			textFieldInsertionJointError = new JTextField();
			textFieldInsertionJointError.setBounds(new Rectangle(510, 419, 70, 20));
			textFieldInsertionJointError.setEditable(false);
			textFieldInsertionJointError.setColumns(10);
			textFieldInsertionJointError.setText("0");
		}
		return textFieldInsertionJointError;
	}

	/**
	 * This method initializes textFieldRotationJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldRotationJointError() {
		if (textFieldRotationJointError == null) {
			textFieldRotationJointError = new JTextField();
			textFieldRotationJointError.setBounds(new Rectangle(510, 451, 70, 20));
			textFieldRotationJointError.setEditable(false);
			textFieldRotationJointError.setColumns(10);
			textFieldRotationJointError.setText("0");
		}
		return textFieldRotationJointError;
	}

	/**
	 * This method initializes textFieldRetractionJointError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldRetractionJointError() {
		if (textFieldRetractionJointError == null) {
			textFieldRetractionJointError = new JTextField();
			textFieldRetractionJointError.setBounds(new Rectangle(510, 483, 70, 20));
			textFieldRetractionJointError.setEditable(false);
			textFieldRetractionJointError.setColumns(10);
			textFieldRetractionJointError.setText("0");
		}
		return textFieldRetractionJointError;
	}

	/**
	 * This method initializes jButtonMoveTo
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonInsertNeedle() {
		if (jButtonInsertNeedle == null) {
			jButtonInsertNeedle = new JButton();
			jButtonInsertNeedle.addActionListener(new ActionListener() {
				public void actionPerformed(ActionEvent e) {
					if (isEnableInsertion() == false) {
						setEnableInsertion(true);
						jButtonInsertNeedle.setText("Stop insertion");
	
						desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
						desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
						
						desiredJointTicksInteger[2] = 0;						
						// only update insertion axis
						desiredJointTicksInteger[3] = (int) Math.floor(desiredJointTicksDouble[3]);
						desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[4];
						runDesiredJointUpdate(2);	
						
						//setInsertionFinished(true);
						
					} else {
						setEnableInsertion(false);
						jButtonInsertNeedle.setText("Start Insertion");
						desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
						desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
						desiredJointTicksInteger[2] = 0;						
						desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[3];
						desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[4];
						runDesiredJointUpdate(0.01);
					}
				}
			});
			jButtonInsertNeedle.setBounds(new Rectangle(290, 568, 135, 25));
			jButtonInsertNeedle.setText("Insertion Needle");
			jButtonInsertNeedle.setActionCommand("");
		}
		return jButtonInsertNeedle;
	}

	/**
	 * This method initializes jButtonStop
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonStop() {
		if (jButtonStop == null) {
			jButtonStop = new JButton();
			jButtonStop.setFont(new Font("Tahoma", Font.BOLD, 16));
			jButtonStop.setBounds(new Rectangle(649, 533, 80, 60));
			jButtonStop.setForeground(Color.red);
			jButtonStop.setText("STOP");
			jButtonStop.addActionListener(new ActionListener() {
				public void actionPerformed(ActionEvent e) {

					System.out.println("Stopping all motors!");
					getDevice().SetAllPIDSetPoint(
							getKinematicsGUI().getCurrentJointTicksValues(), 0);
				}
			});
		}
		return jButtonStop;
	}

	/**
	 * This method initializes textFieldXAxisNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldXAxisNeedleCartesianError() {
		if (textFieldXAxisNeedleCartesianError == null) {
			textFieldXAxisNeedleCartesianError = new JTextField();
			textFieldXAxisNeedleCartesianError.setBounds(new Rectangle(643, 334, 100, 20));
			textFieldXAxisNeedleCartesianError.setEditable(false);
			textFieldXAxisNeedleCartesianError.setColumns(10);
			textFieldXAxisNeedleCartesianError.setText("0");
		}
		return textFieldXAxisNeedleCartesianError;
	}

	/**
	 * This method initializes textFieldYAxisNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldYAxisNeedleCartesianError() {
		if (textFieldYAxisNeedleCartesianError == null) {
			textFieldYAxisNeedleCartesianError = new JTextField();
			textFieldYAxisNeedleCartesianError.setBounds(new Rectangle(643, 359, 100, 20));
			textFieldYAxisNeedleCartesianError.setEditable(false);
			textFieldYAxisNeedleCartesianError.setColumns(10);
			textFieldYAxisNeedleCartesianError.setText("0");
		}
		return textFieldYAxisNeedleCartesianError;
	}

	/**
	 * This method initializes textFieldZAxisNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldZAxisNeedleCartesianError() {
		if (textFieldZAxisNeedleCartesianError == null) {
			textFieldZAxisNeedleCartesianError = new JTextField();
			textFieldZAxisNeedleCartesianError.setBounds(new Rectangle(643, 392, 100, 19));
			textFieldZAxisNeedleCartesianError.setEditable(false);
			textFieldZAxisNeedleCartesianError.setColumns(10);
			textFieldZAxisNeedleCartesianError.setText("0");
		}
		return textFieldZAxisNeedleCartesianError;
	}

	/**
	 * This method initializes textFieldRollNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldRollNeedleCartesianError() {
		if (textFieldRollNeedleCartesianError == null) {
			textFieldRollNeedleCartesianError = new JTextField();
			textFieldRollNeedleCartesianError.setBounds(new Rectangle(643, 422,
					100, 20));
			textFieldRollNeedleCartesianError.setEditable(false);
			textFieldRollNeedleCartesianError.setColumns(10);
			textFieldRollNeedleCartesianError.setText("0");
		}
		return textFieldRollNeedleCartesianError;
	}

	/**
	 * This method initializes textFieldXPitchNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldXPitchNeedleCartesianError() {
		if (textFieldXPitchNeedleCartesianError == null) {
			textFieldXPitchNeedleCartesianError = new JTextField();
			textFieldXPitchNeedleCartesianError.setBounds(new Rectangle(643,
					454, 100, 20));
			textFieldXPitchNeedleCartesianError.setEditable(false);
			textFieldXPitchNeedleCartesianError.setColumns(10);
			textFieldXPitchNeedleCartesianError.setText("0");
		}
		return textFieldXPitchNeedleCartesianError;
	}

	/**
	 * This method initializes textFieldYawNeedleCartesianError
	 * 
	 * @return javax.swing.JTextField
	 */
	private JTextField getTextFieldYawNeedleCartesianError() {
		if (textFieldYawNeedleCartesianError == null) {
			textFieldYawNeedleCartesianError = new JTextField();
			textFieldYawNeedleCartesianError.setBounds(new Rectangle(643, 486,
					100, 20));
			textFieldYawNeedleCartesianError.setEditable(false);
			textFieldYawNeedleCartesianError.setColumns(10);
			textFieldYawNeedleCartesianError.setText("0");
		}
		return textFieldYawNeedleCartesianError;
	}

	/**
	 * This method initializes jButtonRetractNeedle
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonRetractNeedle() {
		if (jButtonRetractNeedle == null) {
			jButtonRetractNeedle = new JButton();
			jButtonRetractNeedle.addActionListener(new ActionListener() {
				public void actionPerformed(ActionEvent e) {
						
					if (isEnableRetraction() == false) {
						setEnableRetraction(true);
						jButtonRetractNeedle.setText("Stop retraction");
	
						desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
						desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
						
						desiredJointTicksInteger[2] = 0;
						// only update insertion axis
						desiredJointTicksInteger[3] = 0;	
						desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[4];
						
						runDesiredJointUpdate(2);						
					} else {
						setEnableRetraction(false);
						jButtonRetractNeedle.setText("Start retraction");
						desiredJointTicksInteger[0] = getKinematicsGUI().getCurrentJointTicksValues()[0];
						desiredJointTicksInteger[1] = getKinematicsGUI().getCurrentJointTicksValues()[1];
						desiredJointTicksInteger[2] = 0;						
						desiredJointTicksInteger[3] = getKinematicsGUI().getCurrentJointTicksValues()[3];
						desiredJointTicksInteger[4] = getKinematicsGUI().getCurrentJointTicksValues()[4];
						
						runDesiredJointUpdate(0.01);
					}
					
					
				}
			});
			jButtonRetractNeedle.setBounds(new Rectangle(435, 533, 120, 25));
			jButtonRetractNeedle.setText("Retract Needle");
		}
		return jButtonRetractNeedle;
	}

	/**
	 * This method initializes jButtonRetractStylet
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonRetractStylet() {
		if (jButtonRetractStylet == null) {
			jButtonRetractStylet = new JButton();
			jButtonRetractStylet.addActionListener(new ActionListener() {
				public void actionPerformed(ActionEvent e) {
				}
			});
			jButtonRetractStylet.setBounds(new Rectangle(435, 568, 120, 25));
			jButtonRetractStylet.setText("Retract Stylet");
		}
		return jButtonRetractStylet;
	}

	/**
	 * This method initializes jButtonHomePosition
	 * 
	 * @return javax.swing.JButton
	 */
	private JButton getJButtonHomePosition() {
		if (jButtonHomePosition == null) {
			jButtonHomePosition = new JButton();
			jButtonHomePosition.addActionListener(new ActionListener() {
				public void actionPerformed(ActionEvent e) {
					setEnableHomePosition(true);

					desiredJointTicksInteger[0] = 0;
					desiredJointTicksInteger[1] = 0;
					desiredJointTicksInteger[2] = 0;
					desiredJointTicksInteger[3] = 0;
					desiredJointTicksInteger[4] = 0;
					
					runDesiredJointUpdate(2);
					
					if (isEnableHomePosition()==true){
						jButtonHomePosition.setText("Homing");
					}else{
						jButtonHomePosition.setText("Home");
					}
					
				}
			});
			jButtonHomePosition.setBounds(new Rectangle(559, 533, 80, 60));
			jButtonHomePosition.setText("Home");
		}
		return jButtonHomePosition;
	}

	private JButton getJButtonZFrameRegistration() {
		if (jButtonZFrameRegistration == null) {
			jButtonZFrameRegistration = new JButton();
			jButtonZFrameRegistration
					.setBounds(new Rectangle(213, 130, 180, 25));
			jButtonZFrameRegistration.setText("Confirm Registration");
			jButtonZFrameRegistration.addActionListener(new ActionListener() {
				boolean enableTable = true;
				public void actionPerformed(ActionEvent e) {
					if (enableTable==true){
						jButtonZFrameRegistration.setText("Registration Confirmed");
					}
					else
					{
						jButtonZFrameRegistration.setText("Confirm Registration");
					}
					
					enableTable = !enableTable;
					tableZFrameRegistration.setEnabled(enableTable);
				}
			});
			// jButtonStop.addActionListener(new ActionListener() {
			// public void actionPerformed(ActionEvent e) {
			//
			// System.out.println("Stopping all motors!");
			// getDevice().SetAllPIDSetPoint(getKinematicsGUI().getValues(), 0);
			// }
			// });

		}
		return jButtonZFrameRegistration;
	}

	private JButton getjButtonDesiredNeedleRAS() {
		if (jButtonDesiredNeedleRAS == null) {
			jButtonDesiredNeedleRAS = new JButton();
			// setBounds(int x, int y, int width, int height)
			jButtonDesiredNeedleRAS.setBounds(new Rectangle(17, 130, 185, 25));
			jButtonDesiredNeedleRAS.setText("Confirm Needle Position");
			jButtonDesiredNeedleRAS.addActionListener(new ActionListener() {
			boolean enableTable = true;

				public void actionPerformed(ActionEvent e) {
					if (enableTable==true){
						jButtonDesiredNeedleRAS.setText("Needle Position Confirmed");
					}
					else
					{
						jButtonDesiredNeedleRAS.setText("Confirm Needle Position");
					}					
					// every click would switch to enable/disable the table
					enableTable = !enableTable;
					tableDesiredNeedleRAS.setEnabled(enableTable);
				}
			});
		}
		return jButtonDesiredNeedleRAS;
	}

	public void setKinematicsGUI(MainGUI kinematicsGUI) {
		this.kinematicsGUI = kinematicsGUI;
	}

	public GenericPIDDevice getDevice() {
		return kinematicsGUI.getDevice();
	}

	public MainGUI getKinematicsGUI() {
		return kinematicsGUI;
	}

	public void setEnableRobotMove(boolean enableRobotMove) {
		this.enableRobotMove = enableRobotMove;
	}

	public boolean isEnableRobotMove() {
		return enableRobotMove;
	}

	public void setEnableHomePosition(boolean enableHomePosition) {
		this.enableHomePosition = enableHomePosition;
	}

	public boolean isEnableHomePosition() {
		return enableHomePosition;
	}

	public void setEnableInsertion(boolean enableInsertion) {
		this.enableInsertion = enableInsertion;
	}

	public boolean isEnableInsertion() {
		return enableInsertion;
	}

	public void setEnableRetraction(boolean enableRetraction) {
		this.enableRetraction = enableRetraction;
	}

	public boolean isEnableRetraction() {
		return enableRetraction;
	}

	public void setEnableRotation(boolean enableRotation) {
		this.enableRotation = enableRotation;
	}

	public boolean isEnableRotation() {
		return enableRotation;
	}

	public void setInsertionVelControlEnabled(boolean insertionVelControlEnabled) {
		this.insertionVelControlEnabled = insertionVelControlEnabled;
	}

	public boolean isInsertionVelControlEnabled() {
		return insertionVelControlEnabled;
	}

	public void setInsertionVel(double insertionVel) {
		this.desiredInsertionVel = insertionVel;
	}

	public double getInsertionVel() {
		return desiredInsertionVel;
	}

	public void setRobotKinematics(ProstateRobotKinematicModel robotKinematics) {
		this.robotKinematics = robotKinematics;
	}

	public ProstateRobotKinematicModel getRobotKinematics() {
		return robotKinematics;
	}
	
	/**
	 * Run desired joint update.
	 * 
	 * @param seconds
	 *            the seconds
*/
	public synchronized void runDesiredJointUpdate(double seconds) {
		// 1 step: update desired joint
		// reference the jointSpaceVector (integer: encoder ticks)
		setDesiredJointTickValue(0,desiredJointTicksInteger[0]-calibrateX);
		setDesiredJointTickValue(1,desiredJointTicksInteger[1]-calibrateY);
		setDesiredJointTickValue(2,desiredJointTicksInteger[2]);
		setDesiredJointTickValue(3,desiredJointTicksInteger[3]-calibrateInsertion);	
		setDesiredJointTickValue(4,desiredJointTicksInteger[4]-calibrateRotation);	
		
		System.err.println("Updating motors");
		// disable Update when updating is finished
		setUpdate(false);
		getDevice().SetPIDSetPoint(0, getDesiredJointTicksValues()[0],seconds);
		getDevice().SetPIDSetPoint(1, getDesiredJointTicksValues()[1],seconds);
		getDevice().SetPIDSetPoint(2, getDesiredJointTicksValues()[2],seconds);
		
		// insertion axis velocity control
		if (isInsertionVelControlEnabled()==true){
			try {
				getDevice().SetPIDSetPoint(3, getDesiredJointTicksValues()[3],getDesiredJointTicksValues()[3]/desiredInsertionVel);
			} catch (ArithmeticException  e) {
				System.err.println("Division by zero!");
			}
		}
		else
		{
		getDevice().SetPIDSetPoint(3, getDesiredJointTicksValues()[3],seconds);	
		}	
		
	}
	
	
	public synchronized void runNeedleRotationVelocity(int velocity, double seconds ) {
		try {
			System.out.println("Running needle rotation");
			getDevice().ResetPIDChannel(4,0);
			getDevice().SetPIDInterpolatedVelocity(4, velocity, seconds);
		} catch (PIDCommandException e) {
			e.printStackTrace();
		}
	}

	/**
	 * Sets the desired joint tick value, specify group number and assign desired joint value to one group (single joint)
	 * 
	 * @param group
	 *            the group
	 * @param target
	 *            the target
	 */
	public void setDesiredJointTickValue(int group, int target) {
		setUpdate(true);
		getDesiredJointTicksValues()[group] = target;
	}
	
	/**
	 * ** get desired joint ticks values.
	 * 
	 * @return integer array of desired joint ticks
	 */
	public int[] getDesiredJointTicksValues() {
		return desiredJointTicks;
	}


	/**
	 * Sets the desired joint ticks values, specify group number and assign desired joint value to all joints
	 * 
	 * @param desiredJointValues
	 *            the new desired joint ticks values
	 */
	public void setDesiredJointTicksValues(int[] desiredJointValues) {
		setUpdate(true);
		this.desiredJointTicks = desiredJointValues;
	}
	
	/**
	 * this field keeps track of if joint values have been changed.
	 * 
	 * @param update
	 *            the new update
	 */
	public void setUpdate(boolean update) {
		this.update = update;
	}

	public void setRotationVelControlEnabled(boolean rotationVelControlEnabled) {
		this.rotationVelControlEnabled = rotationVelControlEnabled;
	}

	public boolean isRotationVelControlEnabled() {
		return rotationVelControlEnabled;
	}
	
	public EventResponseTable getTableDesiredNeedleRAS() {
		return tableDesiredNeedleRAS;
		}

	public void setOutOfRange(boolean isOutRange) {
		this.isOutRange = isOutRange;
	}

	public boolean isOutRange() {
		return isOutRange;
	}

	public void setDesiredInsertionAccel(double desiredInsertionAccel) {
		this.desiredInsertionAccel = desiredInsertionAccel;
	}

	public double getDesiredInsertionAccel() {
		return desiredInsertionAccel;
	}

	public void setDesiredRotationAngle(double desiredRotationAngle) {
		this.desiredRotationAngle = desiredRotationAngle;
	}

	public double getDesiredRotationAngle() {
		return desiredRotationAngle;
	}

	public void setDesiredRotationVel(double desiredRotationVel) {
		this.desiredRotationVel = desiredRotationVel;
	}

	public double getDesiredRotationVel() {
		return desiredRotationVel;
	}

	public void setInsertionFinished(boolean insertionFinished) {
		this.insertionFinished = insertionFinished;
	}

	public boolean isInsertionFinished() {
		return insertionFinished;
	}
}
